﻿#include "safelocker.h"

#include "component_log.h"
#include "componentmanager.h"
#include "systemconfiguration.h"
#include "robot\robot_data.h"


#define LOCK ("1")

SafeLocker::SafeLocker(QObject *parent)
	: ComponentBase(parent)
{
	m_name = "安全锁组件";
	m_type = Component::component_alarmLight;
}

SafeLocker::~SafeLocker()
{
}

// 组件初始化处理函数
int SafeLocker::ComponentInit()
{
	auto cfg = syscfg->GetSafeLockerSetting();
	if (!cfg.enable) {
		log_trace("安全锁配置未使能，无需加载安全锁组件");
		return COMMON_ERROR;
	}
	log_trace("安全锁配置： DI:{}, DO:{}", cfg.state, cfg.set);

	if (cfg.set <= 0 || cfg.set > 32 || cfg.state <= 0 || cfg.state > 32) {
		SetErrmsg("安全锁配置异常，加载失败", false);
		return SERIOUS_ERROR;
	}

	m_infoDi = cfg.state;
	m_setDo = cfg.set;

	m_rob = component->GetComponent(Component::component_robot);
	if (m_rob == nullptr) {
		SetErrmsg("获取机械手组件失败，安全锁组件加载失败", false);
		return SERIOUS_ERROR;
	}

	m_rob->RegisterSubscriberFunction((int)RobotMsgType::robot_msg_DI, [&](const QByteArray& data) {
		return OnReciverDiInfo(data);
		});

	component->RegisterSystemEventCallBake([&](int ev) {
		OnReciverEnterEvent(ev);
		});

	return 0;
}
/*自检接口*/
bool SafeLocker::ComponentCheckSelfState()
{
	if (!m_state) {
		SetErrmsg("安全锁未上锁");
		return false;
	} else {
		return true;
	}
}
/*复位接口*/
bool SafeLocker::ComponentSelfReset()
{
	return CloseLocker();
}

void SafeLocker::ResponseSystemState(int state)
{

}

bool SafeLocker::OpenLocker()
{
	SetDOParam param = {};
	param.index = m_setDo;
	param.val = 1;
	QByteArray in;
	if (!m_rob->ExcuteCmd((int)RobotCmdType::robot_cmd_set_do, param.toByteArray(), in)) {
		SetErrmsg("打开安全锁失败," + in);
		return false;
	}
	return true;
}
bool SafeLocker::CloseLocker()
{
	SetDOParam param = {};
	param.index = m_setDo;
	param.val = 0;
	QByteArray in;
	if (!m_rob->ExcuteCmd((int)RobotCmdType::robot_cmd_set_do, param.toByteArray(), in)) {
		SetErrmsg("关闭安全锁失败," + in);
		return false;
	}
	return true;
}


int SafeLocker::OnReciverDiInfo(const QByteArray& info)
{
	QString di = info.at(m_infoDi - 1);
	bool state = false;
	if (di == LOCK) {
		state = true;
	} else {
		state = false;
	}
	if (state != m_state) {
		m_state = state;
		if (!m_state) {
			SetErrmsg("安全门锁已经开启");
		} else {
			log_trace("安全门锁已关闭");
		}
	}
	return 0;
}

void SafeLocker::OnReciverEnterEvent(int ev)
{
	if (ev == (int)ComponentEvent::system_event_Enter) {
		SetWarningmsg("收到请求进入事件，打开安全门");
		OpenLocker();
	}
}
